import time
import sim  # 导入CoppeliaSim Python API

joint_name = 'joint1'


def connect_to_coppeliasim(port=19999):
    sim.simxFinish(-1)  # 关闭所有连接
    clientID = sim.simxStart('127.0.0.1', port, True, True, 5000, 5)
    if clientID != -1:
        print('Connected to CoppeliaSim')
    else:
        raise Exception('Failed to connect to CoppeliaSim')
    return clientID

# y方向反向移动函数
def move_y_negative(clientID, object_name, velocity, duration):
    # 获取物体句柄
    _, handle = sim.simxGetObjectHandle(clientID, object_name, sim.simx_opmode_blocking)
    
    if handle == -1:
        raise Exception(f'Failed to get handle for object: {object_name}')
    
    # 向左移动（在x轴上负方向）
    for _ in range(duration):
        # 获取当前位置
        _, position = sim.simxGetObjectPosition(clientID, handle, -1, sim.simx_opmode_blocking)
        
        # 更新位置
        new_position = [position[0] , position[1]-velocity, position[2]]
        sim.simxSetObjectPosition(clientID, handle, -1, new_position, sim.simx_opmode_oneshot)
        
        time.sleep(0.1)
def move_y_positive(clientID, object_name, velocity, duration):
    _, handle = sim.simxGetObjectHandle(clientID, object_name, sim.simx_opmode_blocking)

    if handle == -1:
        raise Exception(f'Failed to get handle for object: {object_name}')

    for _ in range(duration):
        _, position = sim.simxGetObjectPosition(clientID, handle, -1, sim.simx_opmode_blocking)
        new_position = [position[0], position[1]+velocity, position[2]]
        sim.simxSetObjectPosition(clientID, handle, -1, new_position, sim.simx_opmode_oneshot)

def move_x_negative(clientID, object_name, velocity, duration):
    _, handle = sim.simxGetObjectHandle(clientID, object_name, sim.simx_opmode_blocking)

    if handle == -1:
        raise Exception(f'Failed to get handle for object: {object_name}')

    for _ in range(duration):
        _, position = sim.simxGetObjectPosition(clientID, handle, -1, sim.simx_opmode_blocking)
        new_position = [position[0]-velocity, position[1], position[2]]
        sim.simxSetObjectPosition(clientID, handle, -1, new_position, sim.simx_opmode_oneshot)

def move_x_positive(clientID, object_name, velocity, duration):
    _, handle = sim.simxGetObjectHandle(clientID, object_name, sim.simx_opmode_blocking)

    if handle == -1:
        raise Exception(f'Failed to get handle for object: {object_name}')

    for _ in range(duration):
        _, position = sim.simxGetObjectPosition(clientID, handle, -1, sim.simx_opmode_blocking)
        new_position = [position[0]+velocity, position[1], position[2]]
        sim.simxSetObjectPosition(clientID, handle, -1, new_position, sim.simx_opmode_oneshot)

def move_z_negative(clientID, object_name, velocity, duration):
    _, handle = sim.simxGetObjectHandle(clientID, object_name, sim.simx_opmode_blocking)

    if handle == -1:
        raise Exception(f'Failed to get handle for object: {object_name}')

    for _ in range(duration):
        _, position = sim.simxGetObjectPosition(clientID, handle, -1, sim.simx_opmode_blocking)
        new_position = [position[0], position[1], position[2]-velocity]
        sim.simxSetObjectPosition(clientID, handle, -1, new_position, sim.simx_opmode_oneshot)

def move_z_positive(clientID, object_name, velocity, duration):
    _, handle = sim.simxGetObjectHandle(clientID, object_name, sim.simx_opmode_blocking)

    if handle == -1:
        raise Exception(f'Failed to get handle for object: {object_name}')

    for _ in range(duration):
        _, position = sim.simxGetObjectPosition(clientID, handle, -1, sim.simx_opmode_blocking)
        new_position = [position[0], position[1], position[2]+velocity]
        sim.simxSetObjectPosition(clientID, handle, -1, new_position, sim.simx_opmode_oneshot)


def main():
    clientID = connect_to_coppeliasim()
    
    try:
        move_y_positive(clientID, joint_name, 0.001, 10)  # 控制joint1向左移动
        time.sleep(0.5)
        move_y_negative(clientID, joint_name, 0.001, 10)  # 控制joint1向右移动
        time.sleep(0.5) 
        move_x_positive(clientID, joint_name, 0.001, 10)  # 控制joint1向前移动
        time.sleep(0.5)
        move_x_negative(clientID, joint_name, 0.001, 10)  # 控制joint1向后移动
        time.sleep(0.5)
        move_z_positive(clientID, joint_name, 0.001, 10)  # 控制joint1向上移动
        time.sleep(0.5)
        move_z_negative(clientID, joint_name, 0.001, 10)  # 控制joint1向下移动

    finally:
        # 关闭连接
        sim.simxFinish(clientID)

if __name__ == '__main__':
    main()
